package com.example.gyroscopeprotocol

import android.os.Bundle
import android.util.Log
import androidx.appcompat.app.AppCompatActivity
import com.example.gyroscopeprotocol.databinding.ActivityMainBinding
import com.example.protocol.ActCode
import com.example.protocol.GyroscopeController
import com.example.protocol.IObserver
import java.util.*

class MainActivity : AppCompatActivity(), IObserver {

    private lateinit var binding: ActivityMainBinding
    private val TAG = "MainActivity"

    override fun onCreate(savedInstanceState: Bundle?) {
        super.onCreate(savedInstanceState)

        binding = ActivityMainBinding.inflate(layoutInflater)
        setContentView(binding.root)

        GyroscopeController.Builder()
            .configPortName("/dev/ttyO4")
            .configBaud(115200)
            .configObserver(this)
            .build()
    }


    override fun dataNotify(@ActCode code: Int, data: ByteArray) {
        when (code) {
            ActCode.ACT_01 -> {
                /**
                 * 姿态角
                 * 滚转角（X 轴） Roll(float) = (float) ( (int16_t) (RollH<<8) | RollL ) / 32768 * 180 (°)
                 * 俯仰角（Y 轴） Pitch(float) = (float) ( (int16_t) (PitchH<<8) | PitchL ) / 32768 * 180 (°)
                 * 偏航角（Z 轴） Yaw(float) = (float) ( (int16_t) (YawH<<8) | YawL ) / 32768 * 180 (°)
                 */
                val RollL = data[4].toInt()
                val RollH = data[5].toInt()
                val PitchL = data[6].toInt()
                val PitchH = data[7].toInt()
                val YawL = data[8].toInt()
                val YawH = data[9].toInt()

                val Roll = ((RollH shl 8) or RollL).toFloat() / 32768 * 180
                val Pitch = ((PitchH shl 8) or PitchL).toFloat() / 32768 * 180
                val Yaw = ((YawH shl 8) or YawL).toFloat() / 32768 * 180

                val SUM = (0x55 + 0x55 + 0x01 + 0x06 + RollL + RollH + PitchL + PitchH + YawL + YawH).toByte()
//                Log.d(TAG, "dataNotify: ${data.contentToString()}  SUM = $RollL + $RollH + $PitchL + $PitchH + $YawL + $YawH = $SUM")

                if (SUM == data[data.size - 1]) {
                    Log.d(TAG, "姿态角: \t 滚转角（X 轴）:$Roll \t 俯仰角（Y 轴）:$Pitch \t 偏航角（Z 轴）:$Yaw")
                } else Log.d(TAG, "dataNotify: 姿态角错误！$SUM != ${data[data.size-1].toInt()}")

            }
            ActCode.ACT_02 -> {

                /**
                 * 四元数
                 * q0(float) = (float) ( (int16_t) (Q0H<<8) | Q0L) / 32768 （范围 0.0~1.0）
                 * q1(float) = (float) ( (int16_t) (Q1H<<8) | Q1L) / 32768 （范围 0.0~1.0）
                 * q2(float) = (float) ( (int16_t) (Q2H<<8) | Q2L) / 32768 （范围 0.0~1.0）
                 * q3(float) = (float) ( (int16_t) (Q3H<<8) | Q3L) / 32768 （范围 0.0~1.0）
                 */
                val Q0L = data[4].toInt()
                val Q0H = data[5].toInt()
                val Q1L = data[6].toInt()
                val Q1H = data[7].toInt()
                val Q2L = data[8].toInt()
                val Q2H = data[9].toInt()
                val Q3L = data[10].toInt()
                val Q3H = data[11].toInt()
                val q0 = (Q0H shl 8 or Q0L).toFloat() / 32768
                val q1 = (Q1H shl 8 or Q1L).toFloat() / 32768
                val q2 = (Q2H shl 8 or Q2L).toFloat() / 32768
                val q3 = (Q3H shl 8 or Q3L).toFloat() / 32768

                val SUM = ((0x55 + 0x55 + 0x02 + 0x08 + Q0L + Q0H + Q1L + Q1H + Q2L + Q2H + Q3L + Q3H)).toByte()
                if (SUM == data[data.size - 1]) {
                    Log.d(
                        TAG,
                        "q0 = $q0 （范围 0.0~1.0）\t" +
                                "q1 = $q1 （范围 0.0~1.0）\t" +
                                "q2 = $q2 （范围 0.0~1.0）\t" +
                                "q3 = $q3 （范围 0.0~1.0"
                    )
                } else Log.d(TAG, "dataNotify: 错误四元数！$SUM != ${data[data.size-1].toInt()}")

            }
            ActCode.ACT_03 -> {
                
            }
            ActCode.ACT_04 -> {
            }
            ActCode.ACT_05 -> {
            }
            ActCode.ACT_06 -> {
            }
            ActCode.ACT_CON_STATUS -> {
            }
        }
    }

}